

clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

vx = 1.89881;
vy = -1.39495;
theta_td = 2.147277;
xinit = -ForceControlModelClass.r0*cos(theta_td);
yinit = ForceControlModelClass.r0*sin(theta_td);

% fp_options = optimset('Display','iter','TolFun',1e-15, 'TolX',1e-10);

% tic
% td_angle = fsolve(@fp_fun,  evalin('base','first_guess'), fp_options);
% error = fp_fun(td_angle)
% toc

% assignin('base','td_angle',td_angle);

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;


%% Fixed Point function

%     function error = fp_fun(x)

phases = 2;
output = cell(phases, 2);

x0 = [ xinit, vx, yinit, vy, 0, 0, theta_td, 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];

% controller = ComControllerClass;
controller = SimpleControllerClass;

sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);

for i = 1 : phases
    [output{i, 1}, output{i, 2}] = sim.run();
end

% error = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);    % horizontal velocity
%     output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)       ];      % COM height

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;

% end